// -*-C++-*-
/*!
 * @file  LocationInfoSVC_impl.cpp
 * @brief Service implementation code of LocationInfo.idl
 *
 */

#include "LocationInfoSVC_impl.h"
#include "structure.h"
#include "mcl.h"

/*
 * Example implementational code for IDL interface LocationInfo
 */
LocationInfoSVC_impl::LocationInfoSVC_impl(Mcl *mcl_p, Point *pose_mean_p, Point *pose_std_p)
{
  mcl = mcl_p;
  pose_mean = pose_mean_p;
  pose_std  = pose_std_p;
}


LocationInfoSVC_impl::~LocationInfoSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */
CORBA::Boolean LocationInfoSVC_impl::setLocation(CORBA::Double x, CORBA::Double y, CORBA::Double theta)
{
  // 取得位置を初期位置，姿勢として初期化
  pose_mean->x = x;
  pose_mean->y = y;
  pose_mean->theta = theta;
  
  // パーティクル初期化
  localizer_initialize_particles_gaussians(mcl->filter, 1, pose_mean, pose_std);
  
  return 0;
}

CORBA::Boolean LocationInfoSVC_impl::getLocation(CORBA::Double& x, CORBA::Double& y, CORBA::Double& theta)
{
  x = mcl->filter->max_particle.x;
  y = mcl->filter->max_particle.y;
  theta = mcl->filter->max_particle.theta;
  
  return 0;
}



// End of example implementational code



